#pragma once

#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <deque>

#define EPS			0.0001f
#define PI			3.1415926535897932384f
#define PI2			6.2831853071795864769f

using namespace std;

struct PumaPack {
	int find, sind;
	float fang, sang;
	bool aviable, choosen;

	PumaPack() {
		choosen = false;
		aviable = true;
	}
};

struct FindStr {
	int f, s;
	int val;
};

struct Pos {
	float x, y;
};

struct Limmitation {
	float x, y, w, h;

	Limmitation();
	Limmitation(float x, float y, float w, float h);
};

class Puma2C {
private:
	int ** angleSpace;
	int fLength, sLength;
	int find, sind;
	PumaPack remBegin, remEnd;

	inline void NextF(int& index) { if(++index >= fLength) index = 0; }
	inline void PrevF(int& index) { if(--index < 0) index = fLength-1; }
	inline void NextS(int& index) { if(++index >= sLength) index = 0; }
	inline void PrevS(int& index) { if(--index < 0) index = sLength-1; }

	bool Include(float a, float b, float x);
	bool CheckIfCross(float x1, float y1, float x2, float y2,
		float x3, float y3, float x4, float y4);
	bool CalculateCrossPoint(float A1, float B1, float C1, float A2, float B2, float C2, float& X, float& Y);
	bool CrossLimmitation(const Limmitation& limit, float A, float B, float C);
	void CalculateLine(float x1, float y1, float x2, float y2, float& A, float& B, float& C);
	bool AngleValue(PumaPack& sol);
protected:
	float precOfFirstAngle, precOfSecAngle;
	float firstAngle, secAngle;
	float firstLength, secondLength;
	deque<Limmitation> limmits;
	
	float CheckIndex(int f, int s, Pos p);
	void clearSpaceTab();
	void eraseSpaceTab();
	void reinitSpaceTab();
	void freeSpaceTab();
	bool CrossLimmitation(const Limmitation& limit, float fAng, float sAng);
public:
	Puma2C(float fPrec = (PI2 / 180.0f), float sPrec = (PI2 / 180.0f));
	~Puma2C();

	inline deque<Limmitation>* GetLimmitDeque() { return &limmits; }
	inline int		GetSLength() { return sLength; }
	inline int		GetFLength() { return fLength; }
	inline float	GetFPrec() { return precOfFirstAngle; }
	inline float	GetSPrec() { return precOfSecAngle; }
	inline bool		IfAviable(int f, int s) 
					{ return !(!angleSpace[s][f]); }
	inline int		GetTabValue(int f, int s) 
					{ return angleSpace[s][f]; }
	inline int		GetSInd() { return sind; }
	inline int		GetFInd() { return find; }
	inline PumaPack GetBeginPack() { return remBegin; }
	inline PumaPack GetEndPack() { return remEnd; }

	void	SetAngles(float first, float second);
	void	GetAngles(float* first, float* second);
	void	SetPrecision(float firstPrec, float secPrec);
	void	GetPrecision(float* firstPrec, float* secPrec);
	void	SetLengths(float firstPrec, float secPrec);
	void	GetLengths(float* firstPrec, float* secPrec);
	void	SetSpaceTabSize(int fSize, int sSize);
	bool	Reverse(float x, float y, PumaPack& sol, PumaPack& sol2);
	void	AddLimit(float x, float y, float w, float h, deque<Pos>* deq = NULL);
	void	UpdateLimit(float x, float y, deque<Pos>* deq = NULL);
	void	UpdatePoints(deque<Pos>* deq);
	void	GetPointsPos(float& Mx, float& My, float& Ex, float& Ey);
	void	CheckSpace(deque<Pos>* deq);
	void	ClearLimmitations(deque<Pos>* deq);
	inline void	ResetPointCloud(deque<Pos>* deq) {
			eraseSpaceTab();
			CheckSpace(deq);
	}

	inline void	SetAsBegin(PumaPack p) { 
								remBegin = p; 
								find = remBegin.find;
								sind = remBegin.sind;
								}
	inline void	SetAsEnd(PumaPack p) { remEnd = p; }
	//inline PumaPack GetEndPack() { return remEnd; }
	bool	FindPath();
	bool	FindPath(PumaPack beg, PumaPack end);
	void	NextStep(float& fang, float& sang);

	//testowo
	void	CrossLimmitation(float fAng, float sAng);
};